/// @file usbsensor.cpp
/// @brief Implementation of the Thread class
/// @date 23/09/2009
/// @author Jérôme Stadelmann, Reds, Heig-vd

#include "usbsensor.h"
#include <iostream>
#include <stdio.h>

using namespace std;

//*************************************************************************************************
// Constructor
//*************************************************************************************************
UsbSensor::UsbSensor()
{
    m_hDevice = -1;
}

//*************************************************************************************************
// Destructor
//*************************************************************************************************
UsbSensor::~UsbSensor()
{
    if (m_hDevice > -1)
    {
        // Unset led blink mode
        OakFeatureReport ledReport = { 0, 0, 1, 1, 0, 0 };
        sendFeatureReport(m_hDevice, ledReport);

        closeDevice(m_hDevice);
        m_hDevice = -1;
    }

    delete [] m_pBuffers;
}

//*************************************************************************************************
// Function checkStatus
/// @param status The report to check
/// @return True if succeeded
//*************************************************************************************************
bool UsbSensor::checkStatus(EOakStatus status)
{
    if ( eOakStatusOK != status )
    {
        fprintf(stderr, "Sensor Error %d: %s\n", status, getStatusString (status).c_str());
        return false;
    }
    return true;
}

//*************************************************************************************************
// Function checkForSensor
/// @param device The name of the USB HID device
/// @return True if a sensor exists
//*************************************************************************************************
bool UsbSensor::checkForSensor(string device)
{
    int hDevice;
    bool bRet = (eOakStatusOK == openDevice(device, hDevice));
    if (bRet)
        closeDevice(hDevice);

    return bRet;
}

//*************************************************************************************************
// Function initSensor
/// @param device The name of the USB HID device
/// @return True if succeeded
//*************************************************************************************************
bool UsbSensor::initSensor(string device)
{
    CHECK_OAK_STATUS(openDevice(device, m_hDevice));

    DeviceInfo devInfo;
    CHECK_OAK_STATUS(getDeviceInfo(m_hDevice, devInfo));
    cout << "Device name : " << devInfo.deviceName << endl;
    cout << "Volatile device name : " << devInfo.volatileUserDeviceName << endl;

    // Create the buffers
    m_iChannels = devInfo.numberOfChannels;       
    m_pBuffers = new CircularBuffer[m_iChannels]();

    // Get the sensor name
    OakFeatureReport nameReport = { 1, 0, 0x15, 0, 0 };
    CHECK_OAK_STATUS(sendReportAndWaitForReply(m_hDevice, nameReport));
    cout << "Sensor name : " << &nameReport[1] << endl;

    // Set report mode
    OakFeatureReport modeReport = { 0, 0, 1, 0, 0, 0 }; // After sampling
    CHECK_OAK_STATUS(sendFeatureReport(m_hDevice, modeReport));

    // Set led blink mode
    OakFeatureReport ledReport = { 0, 0, 1, 1, 0, 2 };
    CHECK_OAK_STATUS(sendFeatureReport(m_hDevice, ledReport));

    // Set sample rate
    OakFeatureReport rateReport = { 0, 0, 2, 1, 0, SENSOR_PERIOD, 0x00 }; // 10 miliseconds
    CHECK_OAK_STATUS(sendFeatureReport(m_hDevice, rateReport));

    return true;
}

//*************************************************************************************************
// Function getBufferArray
/// @param channel The sensors channel
/// @param array A pointer to the array that will be filled with the sensor data
/// @param size The maximal size of the array
/// @param newOnly True for getting only the last values that have not been read since last call
/// @return The number of values written in the array
//*************************************************************************************************
int UsbSensor::getBufferArray(int channel, short* array, int size, bool newOnly)
{
    if (channel >= m_iChannels || channel < 0)
    {
        cerr << "Error on void UsbSensor::getBufferArray : invalid channel" << endl;
        return 0;
    }

    return m_pBuffers[channel].toArray(array, size, newOnly);
}

//*************************************************************************************************
// Function startCapture
/// @return True if succeeded
//*************************************************************************************************
bool UsbSensor::startCapture()
{
    m_bContinue = true;

    start();
    return true;
}

//*************************************************************************************************
// Function stop
//*************************************************************************************************
void UsbSensor::stop()
{
    m_bContinue = false;
}

//*************************************************************************************************
// Function run
//*************************************************************************************************
void UsbSensor::run()
{
    if (m_hDevice < 0)
        return;

    while (m_bContinue)
    {
        // Read a complete value set from sensor
        int count=0;
        vector<int> values;
        while (count<m_iChannels)
            if (eOakStatusOK == readInterruptReport(m_hDevice, values))
                for (unsigned int i=0; i<values.size() && count<m_iChannels; i++, count++)
                    m_pBuffers[count].append(values[i]);
                //CBT
        //printf("buffercount %d,values %d\n",count,values[i]);}

        usleep(1); // Used to limit the processor load
    };
}
